#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
#  ReadNMEA.py
#
#  Copyright 2016 Martin Schöön <martin.schoon@ieee.org>
#
#  This program is free software; you can redistribute it and/or modify
#  it under the terms of the GNU General Public License as published by
#  the Free Software Foundation; either version 2 of the License, or
#  (at your option) any later version.
#
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
#
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software
#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
#  MA 02110-1301, USA.
#
#

import numpy as np

latf = []
lonf = []
vf = []


def ReadGPSData(infilename):
    """Reads lat, lon and speed from named file that must be NMEA formatted
    a la Sony action cam HDR-AS200V. Returns extracted information as
    three NumPy arrays."""
    ifile = open(infilename, 'r')
    for line in ifile:
        if line.startswith('$GPRMC'):
            try:
                lat, lathemi, lon, lonhemi, v = line.strip().split(',')[3:8]
                if lathemi == "N":
                    latf.append(float(lat))
                else:
                    latf.append(0.0 - float(lat))
                if lonhemi == "E":
                    lonf.append(float(lon))
                else:
                    lonf.append(0.0 - float(lon))
                vf.append(float(v))
            except:
                print('file read error')

    latitudes = np.array(latf)
    longitudes = np.array(lonf)
    speeds = np.array(vf)

    ifile.close()

    return latitudes, longitudes, speeds


def NMEA2NM(lat, lon):
    """NMEA standard format for lat and lon is a little funny with
    degrees and minutes mashed together. This function takes this
    information and returns data in nautical miles. Hence, Lat in NM
    from equator and Lon i NM from zero meridian at Lat! In this way I
    get a reasonable scaling x/y when plotting tracks as long as I am
    only interested in short trips rather than ocean voyages."""

# Lat NMDD.DDD -> LatNM
    latdeg = np.trunc(lat/100.0)  # Moving decimal point 2 positions left
    latNM = 100.0 * (lat/100.0 - latdeg)  # All to the right of . deg/100
    latNM = latdeg*60.0 + latNM  # putting it back together with 60 min/deg.

# Lon NMDD.DDD -> LonNM, to make this look right on video I use the mid
# latitude value to scale longitude values.
    latmid = (latNM.max() + latNM.min())/2.0
    lonscale = np.cos(np.pi * latmid/60.0/180.0)
    londeg = np.trunc(lon/100.0)
    lonNM = 100.0 * (lon/100.0 - londeg)
    lonNM = (londeg*60.0 + lonNM)*lonscale

    return latNM, lonNM
